#!/usr/bin/env python3
# pur_server.py
# 服务器模块：负责通信，同时为程序入口来调用其它模块
import socket
import threading
import numpy as np
import time
import os
import struct
import zlib
import logging
import binascii
from math import dist
import sys
import random
import datetime

# 导入模块
from pur_policy_localD import PursuitPolicy
from pur_ui import PursuitGameUI, QApplication

# ================ 全局开关 ================
IF_VISUALIZE = True  # 是否启动Qt可视化窗口
NAV_GOAL_MAX_INTERVAL = 10  # 最大航点发送间隔(秒)
# ================ 通信协议配置 ================
FRAME_SIZE = 32  # 固定帧大小
VERSION = 0x02  # 协议版本
# 指令类型
COMMAND_TYPES = {
    "ConnectionRequest": 0x01,  # 连接请求 (客户端→服务端)
    "ConnectionResponse": 0x02,  # 连接响应 (服务端→客户端)
    "PositionReport": 0x03,  # 位置报告 (客户端→服务端)
    "NavigationGoal": 0x04,  # 导航目标 (服务端→客户端)
    "Heartbeat": 0x05,  # 心跳包 (双向)
    "CommandAck": 0x06,  # 指令确认 (双向)
    "EmergencyStop": 0x07,  # 紧急停止 (服务端→客户端)
    "StatusReport": 0x08,  # 状态报告 (客户端→服务端)
    "RoleAssignment": 0x09,  # 角色分配 (服务端→客户端)
}
# 状态码定义
STATUS_CODES = {
    "Success": 0x00,
    "GeneralError": 0x01,
    "InvalidCommand": 0x02,
    "CRCError": 0x03,
    "Timeout": 0x04,
    "RobotBusy": 0x05,
}
# 角色定义
ROLES = {"evader": 0x01, "pursuer": 0x02}
# 服务器配置
SERVER_IP = "0.0.0.0"  # 监听所有网络接口
SERVER_PORT = 6001
MAX_ROBOTS = 3
ROBOT_TIMEOUT = 8.0
HEARTBEAT_INTERVAL = 2.0
INF = 255
# 配置日志
logging.basicConfig(
    level=logging.INFO,
    format="%(asctime)s - %(levelname)s - %(message)s",
    handlers=[
        logging.StreamHandler(),
        logging.FileHandler("pursuit_game_server_inc.log"),
    ],
)
logger = logging.getLogger(__name__)


class RobotController:
    def __init__(self, robot_id, robot_name, role, client_socket, addr, server):
        """新增server参数用于事件通知"""
        self.robot_id = robot_id
        self.robot_name = robot_name
        self.role = role  # 'evader' 或 'pursuer'
        self.client_socket = client_socket
        self.addr = addr
        self.current_position = None  # 实际坐标
        self.initial_position = None  # 新增：初始位置记录
        self.current_node = None  # 最近的节点
        self.target_position = None  # 目标坐标
        self.target_node = None  # 目标节点
        self.last_update_time = time.time()
        self.last_heartbeat_time = time.time()
        self.status = "connecting"  # connecting, active, paused, disconnected
        self.active = False
        self.debug_mode = False
        self.server = server  # 保存服务器实例引用
        self.last_target_send_time = time.time()  # [新增] 初始化最后发送航点时间
        self.last_position = None  # 新增：上一步位置，用于距离阈值检查
        # 启动接收线程
        self.receive_thread = threading.Thread(
            target=self.receive_messages, daemon=True
        )
        self.receive_thread.start()
        # 发送角色分配信息
        self.send_role_assignment()

    def build_frame(self, command, payload=None):
        """构建32字节通信帧 (修正后的协议)"""
        if payload is None:
            payload = bytes(14)  # 默认14字节负载
        elif len(payload) > 14:
            payload = payload[:14]  # 截断超长部分
        elif len(payload) < 14:
            payload = payload.ljust(14, b"\x00")  # 填充不足部分
        # 获取当前时间戳(毫秒)
        timestamp = int(time.time() * 1000)
        # 构建帧头部分 (1B版本 + 1B命令 + 4B机器人ID + 8B时间戳)
        header = struct.pack(">BBiQ", VERSION, command, self.robot_id, timestamp)
        frame_data = header + payload
        # 计算CRC32校验码 (对前28字节)
        crc32 = zlib.crc32(frame_data) & 0xFFFFFFFF
        crc_bytes = struct.pack(">I", crc32)
        full_frame = frame_data + crc_bytes
        # 调试输出
        if self.debug_mode:
            logger.debug(
                f"[Robot-{self.robot_id}] 构建帧: CMD={command}, TS={timestamp}"
            )
            logger.debug(f"  Header: {binascii.hexlify(header)}")
            logger.debug(f"  Payload: {binascii.hexlify(payload)}")
            logger.debug(f"  CRC: {crc_bytes.hex()}")
        return full_frame

    def parse_frame(self, data):
        """解析32字节通信帧 (修正后的协议)"""
        if len(data) != FRAME_SIZE:
            logger.error(f"无效帧长度: {len(data)}字节 (预期{FRAME_SIZE}字节)")
            return None
        # 分离数据和校验码
        frame_data = data[:28]  # 前28字节用于CRC计算
        received_crc = struct.unpack(">I", data[28:32])[0]
        # 验证CRC
        calculated_crc = zlib.crc32(frame_data) & 0xFFFFFFFF
        if received_crc != calculated_crc:
            logger.warning(
                f"CRC校验失败! 接收: {received_crc:08X}, 计算: {calculated_crc:08X}"
            )
            if self.debug_mode:
                logger.debug(f"原始数据: {binascii.hexlify(data)}")
                logger.debug(f"帧数据: {binascii.hexlify(frame_data)}")
            return None
        # 解析帧头
        try:
            # 关键修复：只解析前14字节作为帧头
            header_part = frame_data[:14]
            version, command, robot_id, timestamp = struct.unpack(">BBiQ", header_part)
            payload = frame_data[14:28]  # 接下来的14字节是负载
            # 验证协议版本
            if version != VERSION:
                logger.warning(f"协议版本不匹配! 接收: {version}, 预期: {VERSION}")
                return None
            return {
                "version": version,
                "command": command,
                "robot_id": robot_id,
                "timestamp": timestamp,
                "payload": payload,
            }
        except struct.error as e:
            logger.error(f"帧解析错误: {e}")
            return None

    def recv_exact(self, size):
        """接收指定长度的数据，超时返回空字节而不是None"""
        data = b""
        start_time = time.time()
        while len(data) < size:
            try:
                self.client_socket.settimeout(1.0)  # 设置每次接收的超时
                chunk = self.client_socket.recv(size - len(data))
                if not chunk:
                    return None  # 连接真正断开
                data += chunk
            except socket.timeout:
                # 检查总时间是否超过10秒
                if time.time() - start_time > 10:
                    logger.warning(f"接收数据超时超过10秒，断开连接")
                    return None
                # 否则继续等待
                continue
            except BlockingIOError:
                # 非阻塞错误继续等待
                if time.time() - start_time > 8:
                    return None
                time.sleep(0.1)
        return data

    def send_role_assignment(self):
        """发送角色分配信息"""
        role_code = ROLES[self.role]
        payload = struct.pack(">B", role_code) + bytes(13)  # 角色 + 保留 (共14字节)
        frame = self.build_frame(COMMAND_TYPES["RoleAssignment"], payload)
        try:
            self.client_socket.sendall(frame)
            logger.info(f"[Robot-{self.robot_id}] 已分配角色: {self.role}")
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送角色失败: {e}")
            return False

    def update_position(self, x, y):
        """更新位置并检测节点变化（改进：加位置距离阈值触发）"""
        old_position = self.current_position
        self.current_position = (x, y)
        # 新增：记录第一次汇报的位置
        if self.initial_position is None:
            self.initial_position = (x, y)
            logger.info(f"[Robot-{self.robot_id}] 记录初始位置: ({x:.2f}, {y:.2f})")
        old_node = self.current_node
        self.current_node = self.server.policy.position_to_node(
            x, y
        )  # 使用服务器的policy实例
        self.last_update_time = time.time()
        self.status = "active"
        self.last_heartbeat_time = time.time()

        # 节点变化触发 - 只在游戏活跃时触发策略更新
        if (
            old_node != self.current_node
            and self.is_active()
            and self.server.game_active
        ):
            self.server.on_robot_node_changed()  # 通知服务器节点变化
            logger.debug(
                f"[Robot-{self.robot_id}] 节点变化: {old_node} -> {self.current_node}"
            )

        # 新增：位置变化阈值触发（0.05m），防节点不变但微动 - 只在游戏活跃时
        if old_position is not None and self.server.game_active:
            old_x, old_y = old_position
            pos_dist = dist((old_x, old_y), (x, y))
            if pos_dist > 0.05 and self.is_active():
                self.server.on_robot_node_changed()
                logger.debug(f"[Robot-{self.robot_id}] 位置变化: {pos_dist:.3f}m")

    def update_target(self, target_node):
        self.target_node = target_node
        self.target_position = self.server.policy.node_to_position(target_node)
        self.last_update_time = time.time()

    def send_target(self):
        """发送目标位置到客户端"""
        if self.target_position is None or self.client_socket is None:
            return False
        try:
            x, y = self.target_position
            # 负载调整为12字节 (3个float) + 2字节填充
            payload = struct.pack(">fff", x, y, 30) + bytes(2)  # x, y, timeout
            frame = self.build_frame(COMMAND_TYPES["NavigationGoal"], payload)
            self.client_socket.sendall(frame)
            self.last_target_send_time = time.time()  # [修改] 发送成功后更新时间
            # logger.info(f"[Robot-{self.robot_id}] 发送目标: ({x:.2f}, {y:.2f})")
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送目标失败: {e}")
            self.status = "disconnected"
            # 注意：如果发送失败，不更新 last_target_send_time，以便更快重试
            return False

    def send_heartbeat(self):
        """发送心跳包到客户端"""
        if self.client_socket is None:
            return False
        try:
            seq = int(time.time() * 1000) % 1000000
            payload = struct.pack(">i", seq) + bytes(10)  # 序列号 + 保留 (共14字节)
            frame = self.build_frame(COMMAND_TYPES["Heartbeat"], payload)
            self.client_socket.sendall(frame)
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送心跳失败: {e}")
            self.status = "disconnected"
            return False

    def receive_messages(self):
        """接收来自客户端的消息"""
        try:
            while self.status != "disconnected":
                # 接收完整帧
                data = self.recv_exact(FRAME_SIZE)
                if not data:
                    logger.info(f"[Robot-{self.robot_id}] 连接断开")
                    self.status = "disconnected"
                    break
                if data == b"":  # 超时但未断开
                    # 检查心跳超时
                    if time.time() - self.last_heartbeat_time > ROBOT_TIMEOUT:
                        logger.warning(f"[Robot-{self.robot_id}] 心跳超时，断开连接")
                        self.status = "disconnected"
                        break
                    continue
                # 解析数据帧
                frame = self.parse_frame(data)
                if not frame:
                    continue
                # 验证机器人ID
                if frame["robot_id"] != self.robot_id:
                    logger.warning(
                        f"[Robot-{self.robot_id}] 收到错误ID的帧: {frame['robot_id']}"
                    )
                    continue
                # 处理心跳包
                if frame["command"] == COMMAND_TYPES["Heartbeat"]:
                    self.last_heartbeat_time = time.time()
                    # 发送心跳响应
                    self.send_heartbeat()
                    continue
                # 处理位置上报 - 修正为与多机巡线服务器一致的格式
                if frame["command"] == COMMAND_TYPES["PositionReport"]:
                    # 解析位置数据 (x, y, status) - 使用12字节
                    try:
                        # 修改点：使用x, y, status
                        x, y, status = struct.unpack(">fff", frame["payload"][:12])
                        self.update_position(x, y)
                        logger.debug(
                            f"[Robot-{self.robot_id}] 位置更新: ({x:.2f},{y:.2f}), 状态: {status}"
                        )
                    except struct.error as e:
                        logger.error(f"[Robot-{self.robot_id}] 位置报告解析错误: {e}")
                        continue
                # 处理指令确认
                elif frame["command"] == COMMAND_TYPES["CommandAck"]:
                    try:
                        acked_command, status_code = struct.unpack(
                            ">BB", frame["payload"][:2]
                        )
                        status_msg = next(
                            (k for k, v in STATUS_CODES.items() if v == status_code),
                            "Unknown",
                        )
                        logger.debug(
                            f"[Robot-{self.robot_id}] 收到指令确认: 命令={acked_command}, 状态={status_msg}"
                        )
                    except struct.error as e:
                        logger.warning(f"[Robot-{self.robot_id}] 指令确认解析失败: {e}")
                else:
                    logger.warning(
                        f"[Robot-{self.robot_id}] 收到未知指令: {frame['command']}"
                    )
        except (ConnectionResetError, socket.error) as e:
            logger.warning(f"[Robot-{self.robot_id}] 连接异常: {e}")
            self.status = "disconnected"
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 接收消息错误: {e}")
            self.status = "disconnected"
        finally:
            if self.client_socket:
                self.client_socket.close()
                self.client_socket = None

    def get_info(self):
        pos_str = (
            "未知"
            if self.current_position is None
            else f"({self.current_position[0]:.1f}, {self.current_position[1]:.1f})"
        )
        target_str = (
            "未设置"
            if self.target_position is None
            else f"({self.target_position[0]:.1f}, {self.target_position[1]:.1f})"
        )
        node_str = "未映射" if self.current_node is None else str(self.current_node)
        target_node_str = (
            "未设置" if self.target_node is None else str(self.target_node)
        )
        initial_str = (
            "未记录"
            if self.initial_position is None
            else f"({self.initial_position[0]:.1f}, {self.initial_position[1]:.1f})"
        )
        return {
            "id": self.robot_id,
            "name": self.robot_name,
            "role": self.role,
            "position": pos_str,
            "initial_position": initial_str,
            "target": target_str,
            "node": node_str,
            "target_node": target_node_str,
            "status": self.status,
            "last_update": time.time() - self.last_update_time,
            "last_heartbeat": time.time() - self.last_heartbeat_time,
        }

    def is_active(self):
        """检查机器人是否活跃（连接且有位置更新）"""
        if self.status == "disconnected":
            return False
        # 检查心跳超时
        if time.time() - self.last_heartbeat_time > ROBOT_TIMEOUT * 2:
            self.status = "disconnected"
            return False
        return (
            self.status == "active"
            and (time.time() - self.last_update_time) < ROBOT_TIMEOUT
        )

    def disconnect(self):
        """断开机器人连接 (简化处理)"""
        self.status = "disconnected"
        if self.client_socket:
            try:
                self.client_socket.shutdown(socket.SHUT_RDWR)
            except:
                pass
            self.client_socket.close()
            self.client_socket = None
        # 注意：receive_thread 会因 socket 关闭而自然退出，无需显式 join


class MultiRobotServer:
    def __init__(self):
        logger.info("程序启动...")
        logger.info("开始加载图文件...")
        self.policy = PursuitPolicy()
        self.graph = self.policy  # 备份graph到server，便于ui访问
        logger.info("图文件加载完毕，开始加载策略文件...")
        logger.info("策略文件加载完毕，开始通讯初始化...")
        # 机器人存储
        self.evader = None  # 逃逸者
        self.pursuers = {}  # 追捕者字典: pursuer_id -> RobotController
        self.server_socket = None
        # 游戏状态
        self.game_active = False
        self.game_thread = None
        self.game_paused = False
        # 心跳管理
        self.heartbeat_thread = threading.Thread(
            target=self.heartbeat_loop, daemon=True
        )
        self.heartbeat_thread.start()
        # 添加节点变化事件
        self.node_change_event = threading.Event()  # 节点变化事件
        # [新增] 连接管理锁
        self.connection_lock = threading.Lock()
        initial_evader = np.random.randint(0, self.policy.node_num)
        self.policy.init_belief(initial_evader)
        logger.info(f"初始信念设置在节点 {initial_evader}")
        logger.info("通讯初始化完毕")
        # Logbag file initialization
        random_num = random.randint(100000, 999999)
        now = datetime.datetime.now()
        self.logbag_filename = (
            f"logbag_{random_num}_{now.strftime('%Y%m%d_%H_%M_%S')}.log"
        )
        self.logbag_file = open(self.logbag_filename, "w")
        logger.info(f"Logbag file created: {self.logbag_filename}")

    def position_reset(self):
        """重置所有机器人的导航目标到初始位置"""
        logger.info("触发位置重置：所有机器人导航至初始位置")
        # 处理逃逸者
        if self.evader and self.evader.initial_position:
            x, y = self.evader.initial_position
            target_node = self.policy.position_to_node(x, y)
            self.evader.update_target(target_node)
            self.evader.send_target()
            logger.info(
                f"[Robot-{self.evader.robot_id}] 重置目标至初始位置: ({x:.2f}, {y:.2f})"
            )
        # 处理追捕者
        for pursuer in self.pursuers.values():
            if pursuer and pursuer.initial_position:
                x, y = pursuer.initial_position
                target_node = self.policy.position_to_node(x, y)
                pursuer.update_target(target_node)
                pursuer.send_target()
                logger.info(
                    f"[Robot-{pursuer.robot_id}] 重置目标至初始位置: ({x:.2f}, {y:.2f})"
                )

    def toggle_game_active(self):
        """切换游戏暂停/恢复状态"""
        self.game_paused = not self.game_paused
        self.game_active = not self.game_paused
        status = "暂停" if not self.game_active else "恢复"
        logger.info(
            f"游戏{status}，策略更新已{'停止' if not self.game_active else '继续'}"
        )
        if self.game_active:
            self.node_change_event.set()  # 恢复时立即触发一次
        else:  # 暂停时停止导航
            self.send_emergency_stop_to_all()

    def reload_policy(self):
        """重新加载策略和图数据"""
        logger.info("重新加载图和策略...")
        self.policy = PursuitPolicy()
        self.graph = self.policy
        initial_evader = np.random.randint(0, self.policy.node_num)
        self.policy.init_belief(initial_evader)
        logger.info(f"地图重载完成，初始信念设置在节点 {initial_evader}")

    def on_robot_node_changed(self):
        """当机器人节点变化时调用此方法"""
        if self.game_active:
            self.node_change_event.set()  # 触发事件

    def start(self):
        # 创建TCP服务器套接字
        logger.info("正在尝试启动通信接口...")
        logger.info(f"服务器IP: {SERVER_IP}, 端口: {SERVER_PORT}")
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.server_socket.bind((SERVER_IP, SERVER_PORT))
        self.server_socket.listen(5)
        logger.info(f"[Game Server] 正在监听 {SERVER_IP}:{SERVER_PORT}...")
        # 启动状态监控线程
        threading.Thread(target=self.monitor_robots_status, daemon=True).start()
        # 启动游戏主循环
        self.game_thread = threading.Thread(target=self.run_game_loop, daemon=True)
        self.game_thread.start()
        # 接受客户端连接
        self.accept_connections()

    def get_all_robots_info(self):
        """获取所有机器人的状态信息（备份）"""
        robots = []
        if self.evader:
            robots.append(self.evader.get_info())
        for pursuer in self.pursuers.values():
            robots.append(pursuer.get_info())
        return robots

    def get_game_active(self):
        """获取游戏活跃状态（备份）"""
        return self.game_active

    def get_capture_count(self):
        """获取捕获次数（从policy方法获取）"""
        return self.policy.get_capture_count()

    def get_belief_pr(self):
        """获取信念概率（从policy方法获取备份）"""
        return self.policy.get_belief_pr()

    def get_belief_pos(self):
        """获取信念位置（从policy方法获取备份）"""
        return self.policy.get_belief_pos()

    def recv_exact(self, sock, size):
        """接收指定长度的数据，超时返回空字节而不是None"""
        data = b""
        start_time = time.time()
        while len(data) < size:
            try:
                sock.settimeout(1.0)  # 设置每次接收的超时
                chunk = sock.recv(size - len(data))
                if not chunk:
                    return None  # 连接真正断开
                data += chunk
            except socket.timeout:
                # 检查总时间是否超过10秒
                if time.time() - start_time > 10:
                    logger.warning(f"接收连接请求超时超过10秒，断开连接")
                    return None
                # 否则继续等待
                continue
        return data

    def parse_frame(self, data):
        """解析32字节通信帧 (修正后的协议)"""
        if len(data) != FRAME_SIZE:
            logger.error(f"无效帧长度: {len(data)}字节 (预期{FRAME_SIZE}字节)")
            return None
        # 分离数据和校验码
        frame_data = data[:28]  # 前28字节用于CRC计算
        received_crc = struct.unpack(">I", data[28:32])[0]
        # 验证CRC
        calculated_crc = zlib.crc32(frame_data) & 0xFFFFFFFF
        if received_crc != calculated_crc:
            logger.warning(
                f"CRC校验失败! 接收: {received_crc:08X}, 计算: {calculated_crc:08X}"
            )
            return None
        # 解析帧头
        try:
            # 关键修复：只解析前14字节作为帧头
            header_part = frame_data[:14]
            version, command, robot_id, timestamp = struct.unpack(">BBiQ", header_part)
            payload = frame_data[14:28]  # 接下来的14字节是负载
            # 验证协议版本
            if version != VERSION:
                logger.warning(f"协议版本不匹配! 接收: {version}, 预期: {VERSION}")
                return None
            return {
                "version": version,
                "command": command,
                "robot_id": robot_id,
                "timestamp": timestamp,
                "payload": payload,
            }
        except struct.error as e:
            logger.error(f"帧解析错误: {e}")
            return None

    def build_frame(self, command, payload=None, robot_id=0):
        """构建通信帧的通用方法"""
        if payload is None:
            payload = bytes(14)
        elif len(payload) > 14:
            payload = payload[:14]
        elif len(payload) < 14:
            payload = payload.ljust(14, b"\x00")
        timestamp = int(time.time() * 1000)
        header = struct.pack(">BBiQ", VERSION, command, robot_id, timestamp)
        frame_data = header + payload
        crc32 = zlib.crc32(frame_data) & 0xFFFFFFFF
        crc_bytes = struct.pack(">I", crc32)
        return frame_data + crc_bytes

    def accept_connections(self):
        """接受并处理新客户端连接"""
        while True:
            try:
                client_socket, addr = self.server_socket.accept()
                logger.info(f"[Game Server] 新连接来自: {addr}")
                # 设置接收超时
                client_socket.settimeout(2.0)
                # 1. 接收连接请求帧
                data = self.recv_exact(client_socket, FRAME_SIZE)
                if not data:
                    logger.warning("连接未发送数据")
                    client_socket.close()
                    continue
                # 2. 解析连接请求
                frame = self.parse_frame(data)
                if not frame or frame["command"] != COMMAND_TYPES["ConnectionRequest"]:
                    logger.warning("无效的连接请求")
                    client_socket.close()
                    continue
                # 3. 获取机器人名称
                try:
                    robot_name = frame["payload"].rstrip(b"\x00").decode("utf-8")
                except UnicodeDecodeError:
                    robot_name = "unknown_robot"
                logger.info(f"连接请求 | 机器人名称: {robot_name}")
                # 4. 分配机器人ID和角色
                # --- 修改开始：基于槽位是否被占用进行分配 ---
                role, robot_id = None, None
                # 检查逃逸者槽位 (ID 0)
                if self.evader is None:
                    # 如果尚未分配逃逸者，则分配
                    role, robot_id = "evader", 0
                else:
                    # 逃逸者槽位已存在，尝试分配追捕者
                    # 检查追捕者1槽位 (ID 1)
                    if 1 not in self.pursuers:
                        role, robot_id = "pursuer", 1
                    # 检查追捕者2槽位 (ID 2)
                    elif 2 not in self.pursuers:
                        role, robot_id = "pursuer", 2
                    else:
                        # 所有槽位都被占用
                        logger.warning("拒绝连接：无可用位置 (所有角色槽位已满)")
                        client_socket.close()
                        continue
                # --- 修改结束 ---
                if (
                    robot_id is None
                ):  # 这个检查实际上现在不会触发，因为上面逻辑保证了要么分配要么continue
                    logger.warning("拒绝连接：无可用位置")
                    client_socket.close()
                    continue
                # 5. 发送连接响应
                response_payload = struct.pack(">i", robot_id) + bytes(
                    10
                )  # 4字节ID + 10字节填充
                response_frame = self.build_frame(
                    COMMAND_TYPES["ConnectionResponse"], response_payload, robot_id
                )
                client_socket.sendall(response_frame)
                logger.info(f"发送连接响应，分配ID: {robot_id}, 角色: {role}")
                # 6. 创建机器人控制器（新增server参数）
                robot = RobotController(
                    robot_id, robot_name, role, client_socket, addr, self
                )
                # --- 修改：简化连接覆盖逻辑 ---
                # 直接根据分配的角色和ID存储，旧连接的清理由 monitor_robots_status 负责
                # 这可以避免在连接分配过程中因调用 disconnect 而可能产生的竞争或阻塞
                if role == "evader":
                    self.evader = robot
                else:  # role == "pursuer"
                    self.pursuers[robot_id] = robot
                # --- 修改结束 ---
                logger.info(f"机器人 {robot_id} ({role}) 连接成功")
                self.check_game_state()
            except Exception as e:
                logger.error(f"接受连接错误: {e}")
                # 确保 client_socket 在出错时被关闭
                if "client_socket" in locals() and client_socket:
                    try:
                        client_socket.close()
                    except:
                        pass

    def check_game_state(self):
        """检查游戏状态并更新"""
        evader_ready = self.evader and self.evader.is_active()
        pursuer1_ready = 1 in self.pursuers and self.pursuers[1].is_active()
        pursuer2_ready = 2 in self.pursuers and self.pursuers[2].is_active()
        pursuers_ready = pursuer1_ready and pursuer2_ready
        if evader_ready and pursuers_ready:
            if not self.game_active and not self.game_paused:
                logger.info("所有机器人准备就绪，开始围捕!")
                self.game_active = True
                self.node_change_event.set()
                # 游戏开始时，使用evader当前节点初始化信念 (非完美信息)
                if self.evader.current_node is not None:
                    self.policy.init_belief(self.evader.current_node)
                    logger.info(
                        f"游戏开始，信念重置到逃避者节点 {self.evader.current_node}"
                    )
        else:
            if self.game_active:
                logger.warning("机器人断开，暂停围捕!")
                self.game_active = False

    def run_game_loop(self):
        """游戏主循环 - 完全由事件驱动"""
        logger.info("[Game Loop] Game Loop 启动 (事件驱动模式)...")
        while True:
            # 等待节点变化事件
            self.node_change_event.wait()
            if self.game_active:
                self.calculate_and_send_strategy()
            # 重置事件，等待下一次触发
            self.node_change_event.clear()

    def send_emergency_stop_to_all(self):
        """向所有连接的客户端发送紧急停止指令"""
        # 向逃逸者发送紧急停止指令
        if self.evader and self.evader.client_socket:
            try:
                payload = bytes(14)  # 紧急停止不需要额外数据
                frame = self.evader.build_frame(COMMAND_TYPES["EmergencyStop"], payload)
                self.evader.client_socket.sendall(frame)
                logger.info(f"[Robot-{self.evader.robot_id}] 发送紧急停止指令")
            except Exception as e:
                logger.error(
                    f"[Robot-{self.evader.robot_id}] 发送紧急停止指令失败: {e}"
                )

        # 向所有追捕者发送紧急停止指令
        for pursuer in self.pursuers.values():
            if pursuer and pursuer.client_socket:
                try:
                    payload = bytes(14)  # 紧急停止不需要额外数据
                    frame = pursuer.build_frame(COMMAND_TYPES["EmergencyStop"], payload)
                    pursuer.client_socket.sendall(frame)
                    logger.info(f"[Robot-{pursuer.robot_id}] 发送紧急停止指令")
                except Exception as e:
                    logger.error(
                        f"[Robot-{pursuer.robot_id}] 发送紧急停止指令失败: {e}"
                    )

    def calculate_and_send_strategy(self):
        # logger.info("[Game Loop] 进入策略计算和航点发送")
        if not self.game_active:
            return
        if not self.evader or not self.evader.current_position:
            logger.warning("逃逸者未连接或位置未知，跳过策略计算")
            return
        pursuer1 = self.pursuers.get(1)
        pursuer2 = self.pursuers.get(2)
        if (
            not pursuer1
            or not pursuer2
            or not pursuer1.current_position
            or not pursuer2.current_position
        ):
            logger.warning("追捕者未连接或位置未知，跳过策略计算")
            return
        # 获取当前节点
        evader_node = self.evader.current_node
        pursuer1_node = pursuer1.current_node
        pursuer2_node = pursuer2.current_node
        # 检查是否捕获 (真实)
        if self.policy.is_captured(evader_node, pursuer1_node, pursuer2_node):
            logger.info("逃逸者已被捕获！游戏结束")
            self.game_active = False
            self.policy.increment_capture_count()
            self.send_emergency_stop_to_all()
            return
        # 计算下一步 (单步，无预测)
        evader_next, pursuer1_next, pursuer2_next = self.policy.calculate_next_move(
            evader_node, pursuer1_node, pursuer2_node
        )
        logger.info(
            f"[Game Loop] 计算下一步: 逃逸者 {self.evader.current_node}->{evader_next}, \
追捕者1 {pursuer1.current_node}->{pursuer1_next}, 追捕者2 {pursuer2.current_node}->{pursuer2_next}"
        )
        current_time = time.time()
        # [修改] 使用 NAV_GOAL_MAX_INTERVAL 进行强制更新判断
        # 逃逸者检查
        evader_force_update = (
            current_time - self.evader.last_target_send_time
        ) > NAV_GOAL_MAX_INTERVAL
        pursuer1_force_update = (
            current_time - pursuer1.last_target_send_time
        ) > NAV_GOAL_MAX_INTERVAL
        pursuer2_force_update = (
            current_time - pursuer2.last_target_send_time
        ) > NAV_GOAL_MAX_INTERVAL
        force_update = (
            evader_force_update or pursuer1_force_update or pursuer2_force_update
        )
        if evader_next != self.evader.target_node or force_update:
            self.evader.update_target(evader_next)
            self.evader.send_target()
        # 追捕者1检查
        if pursuer1_next != pursuer1.target_node or force_update:
            pursuer1.update_target(pursuer1_next)
            pursuer1.send_target()
        # 追捕者2检查
        if pursuer2_next != pursuer2.target_node or force_update:
            pursuer2.update_target(pursuer2_next)
            pursuer2.send_target()

    def heartbeat_loop(self):
        """定期发送心跳包"""
        while True:
            time.sleep(HEARTBEAT_INTERVAL)
            current_time = time.time()  # [新增] 获取当前时间用于比较
            # 给逃逸者发送心跳 - 始终发送，保持通信
            if self.evader and self.evader.status == "active":
                self.evader.send_heartbeat()
                # [新增] 检查逃逸者是否需要强制更新航点 - 只在游戏活跃时
                if (
                    self.game_active
                    and current_time - self.evader.last_target_send_time
                    > NAV_GOAL_MAX_INTERVAL
                ):
                    logger.info(
                        f"[Heartbeat] 逃逸者 {self.evader.robot_id} 超时未更新航点，触发强制更新。"
                    )
                    # 在新线程中触发，避免阻塞心跳线程
                    threading.Thread(
                        target=self.calculate_and_send_strategy, daemon=True
                    ).start()

            # 给追捕者发送心跳 - 始终发送
            for pursuer in self.pursuers.values():
                if pursuer.status == "active":
                    pursuer.send_heartbeat()
                    # [新增] 检查追捕者是否需要强制更新航点 - 只在游戏活跃时
                    if (
                        self.game_active
                        and current_time - pursuer.last_target_send_time
                        > NAV_GOAL_MAX_INTERVAL
                    ):
                        logger.info(
                            f"[Heartbeat] 追捕者 {pursuer.robot_id} 超时未更新航点，触发强制更新。"
                        )
                        # 在新线程中触发，避免阻塞心跳线程
                        threading.Thread(
                            target=self.calculate_and_send_strategy, daemon=True
                        ).start()

    def monitor_robots_status(self):
        """监控所有机器人状态并定期打印（改进：每4s强制触发策略）"""
        while True:
            time.sleep(4)
            status_report = ["\n===== 机器人状态监控 ====="]
            status_report.append(
                f"游戏状态: {'进行中' if self.game_active else '暂停'}"
            )
            status_report.append(f"捕获次数: {self.policy.get_capture_count()}")
            # 新增：信念摘要
            belief_pr = self.policy.get_belief_pr()
            belief_pos = self.policy.get_belief_pos()
            belief_summary = f"信念支持位置数: {np.sum(belief_pos)}, 熵: {-np.sum(belief_pr[belief_pos] * np.log2(belief_pr[belief_pos] + 1e-10)):.2f}"
            status_report.append(belief_summary)
            if self.evader:
                info = self.evader.get_info()
                status_report.append(f"逃逸者 {info['id']}:")
                status_report.append(
                    f"  位置: {info['position']} | 初始位置: {info['initial_position']} | 节点: {info['node']}"
                )
                status_report.append(
                    f"  目标: {info['target']} | 目标节点: {info['target_node']}"
                )
                status_report.append(
                    f"  状态: {info['status']} | 最后更新: {info['last_update']:.1f}秒前"
                )
                status_report.append(f"  最后心跳: {info['last_heartbeat']:.1f}秒前")
            else:
                status_report.append("逃逸者: 未连接")
            for robot_id in [1, 2]:  # 固定两个追捕者ID
                robot = self.pursuers.get(robot_id)
                if robot:
                    info = robot.get_info()
                    status_report.append(f"追捕者 {info['id']}:")
                    status_report.append(
                        f"  位置: {info['position']} | 初始位置: {info['initial_position']} | 节点: {info['node']}"
                    )
                    status_report.append(
                        f"  目标: {info['target']} | 目标节点: {info['target_node']}"
                    )
                    status_report.append(
                        f"  状态: {info['status']} | 最后更新: {info['last_update']:.1f}秒前"
                    )
                    status_report.append(
                        f"  最后心跳: {info['last_heartbeat']:.1f}秒前"
                    )
                else:
                    status_report.append(f"追捕者 {robot_id}: 未连接")
            status_report.append("=" * 30)
            logger.info("\n".join(status_report))
            # Logbag writing
            timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
            coord_parts = []
            # Evader
            if self.evader and self.evader.current_position:
                ex, ey = self.evader.current_position
            else:
                ex, ey = -1, -1
            coord_parts.append(f"evader:({ex:.2f},{ey:.2f})")
            # Pursuers
            for rid in [1, 2]:
                robot = self.pursuers.get(rid)
                if robot and robot.current_position:
                    px, py = robot.current_position
                else:
                    px, py = -1, -1
                coord_parts.append(f"p{rid}:({px:.2f},{py:.2f})")
            coord_str = " ".join(coord_parts)
            belief_str = ""
            belief_pos_active = np.where(belief_pos)[0]
            if len(belief_pos_active) > 0:
                belief_items = [
                    f"n{node}:{belief_pr[node]:.3f}" for node in belief_pos_active
                ]
                belief_str = " ".join(belief_items)
            line = f"[{timestamp}] {coord_str}"
            if belief_str:
                line += f" [{belief_str}]"
            self.logbag_file.write(line + "\n")
            self.logbag_file.flush()
            # 新增：每4s强制触发策略计算（如果游戏活跃），确保定期更新
            if self.game_active:
                self.node_change_event.set()  # 触发事件
                logger.debug("[Monitor] 定期强制触发策略计算")
            # 清理断开连接的机器人
            if (
                self.evader
                and not self.evader.is_active()
                and self.evader.status == "disconnected"
            ):
                self.evader = None
                logger.info("[清理] 逃逸者已断开连接")
            to_remove = []
            for robot_id, robot in self.pursuers.items():
                if not robot.is_active() and robot.status == "disconnected":
                    to_remove.append(robot_id)
            for robot_id in to_remove:
                del self.pursuers[robot_id]
                logger.info(f"[清理] 追捕者 {robot_id} 已断开连接")
            # 更新游戏状态
            self.check_game_state()


if __name__ == "__main__":
    server = MultiRobotServer()
    import threading

    server_thread = threading.Thread(target=server.start)
    server_thread.daemon = True
    server_thread.start()
    # 根据全局开关决定是否启动Qt窗口
    if IF_VISUALIZE:
        # 启动UI
        app = QApplication(sys.argv)
        ui = PursuitGameUI(server=server, logger=logger)
        ui.show()
        app.exec_()
    else:
        logger.info("Qt可视化窗口已禁用 (IF_VISUALIZE=False)")
        try:
            # 主线程保持运行
            while True:
                threading.Event().wait(0.05)
        except KeyboardInterrupt:
            logger.info("服务器已停止")
            if hasattr(server, "logbag_file"):
                server.logbag_file.close()
